#include "helper.h"
#include <map>

std::map<std::vector<uint16_t>, std::vector<int>>
gobal_helper::Radar_DataProcess::findpeaks(const std::vector<uint16_t>& vec, const uint16_t peak_num, const uint16_t threshold)
{
    std::vector<uint16_t> mask(vec.size(), 0);
    std::vector<uint16_t> peaks;
    std::vector<int> dBs;

    std::map<std::vector<uint16_t>, std::vector<int>> map;

    uint8_t mode = 0;

    if (threshold)
    {
        mode = 1;
    }


    for (int i = 0; i < vec.size() - 2; ++i)
    {
        uint16_t k1 = vec[i];
        uint16_t k2 = vec[i + 1];
        uint16_t k3 = vec[i + 2];
        if ((k2 >= k1 && k2 > k3) || (k2 > k1 && k2 >= k3))
        {
            if (mode)
            {
                if (k2 >= threshold)
                {
                    mask[i + 1] |= 0x01;
                }
                else
                {
                    ++i;
                }
            }
            else
            {
                mask[i + 1] |= 0x01;
            }
        }
        else if ((k1 > k2) && (k2 >= k3))
        {
            ++i;
        }
        else
        {

        }
    }
    //peakcount
    for (uint16_t pkc = 0; pkc < peak_num; ++pkc)
    {
        int maxPeakIndex = -1;
        int maxPeakValue = -1;
        for (int i = 0; i < vec.size(); i++)
        {
            if (mask[i])
            {
                if (vec[i] > maxPeakValue)
                {
                    maxPeakValue = vec[i];
                    maxPeakIndex = i;
                }
            }

        }
        if (maxPeakIndex != -1 && maxPeakValue != -1)
        {
            mask[maxPeakIndex] = 0x00;
            peaks.push_back(maxPeakIndex);
            dBs.push_back(maxPeakValue);
        }
    }
    map.insert(std::make_pair(peaks, dBs));
    return map;

}
